
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       airspeed_calibration.c
  * @author     baiyang
  * @date       2023-8-1
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_airspeed.h"

#include <ahrs/ahrs_view.h>
#include <parameter/param.h>
#include <gcs_mavlink/gcs.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void airspeed_calibration_ctor(airspeed_calibration_t airspeed_cal)
{
    airspeed_cal->P.a.x = 100;
    airspeed_cal->P.a.y = 0;
    airspeed_cal->P.a.z = 0;

    airspeed_cal->P.b.x = 0;
    airspeed_cal->P.b.y = 100;
    airspeed_cal->P.b.z = 0;

    airspeed_cal->P.c.x = 0;
    airspeed_cal->P.c.y = 0;
    airspeed_cal->P.c.z = 0.000001f;

    airspeed_cal->Q0 = 0.01f;
    airspeed_cal->Q1 = 0.0000005f;

    airspeed_cal->state.x = 0;
    airspeed_cal->state.y = 0;
    airspeed_cal->state.z = 0;

    airspeed_cal->DT = 1;
}

/*
  initialise the ratio
 */
void airspeed_calibration_init(airspeed_calibration_t airspeed_cal, float initial_ratio)
{
    airspeed_cal->state.z = 1.0f / sqrtf(initial_ratio);
}

/*
  update the state of the airspeed calibration - needs to be called
  once a second
 */
float airspeed_calibration_update(airspeed_calibration_t airspeed_cal, float airspeed, const Vector3f_t *vg, int16_t max_airspeed_allowed_during_cal)
{
    // Perform the covariance prediction
    // Q is a diagonal matrix so only need to add three terms in
    // C code implementation
    // P = P + Q;
    airspeed_cal->P.a.x += airspeed_cal->Q0;
    airspeed_cal->P.b.y += airspeed_cal->Q0;
    airspeed_cal->P.c.z += airspeed_cal->Q1;

    // Perform the predicted measurement using the current state estimates
    // No state prediction required because states are assumed to be time
    // invariant plus process noise
    // Ignore vertical wind component
    float TAS_pred = airspeed_cal->state.z * math_norm2(vg->x - airspeed_cal->state.x, vg->y - airspeed_cal->state.y, vg->z);
    float TAS_mea  = airspeed;

    // Calculate the observation Jacobian H_TAS
    float SH1 = sq(vg->y - airspeed_cal->state.y) + sq(vg->x - airspeed_cal->state.x);
    if (SH1 < 0.000001f) {
        // avoid division by a small number
        return airspeed_cal->state.z;
    }
    float SH2 = 1/sqrtf(SH1);

    // observation Jacobian
    Vector3f_t H_TAS = {
        -(airspeed_cal->state.z*SH2*(2*vg->x - 2*airspeed_cal->state.x))/2,
        -(airspeed_cal->state.z*SH2*(2*vg->y - 2*airspeed_cal->state.y))/2,
        1/SH2};

    // Calculate the fusion innovation covariance assuming a TAS measurement
    // noise of 1.0 m/s
    // S = H_TAS*P*H_TAS' + 1.0; % [1 x 3] * [3 x 3] * [3 x 1] + [1 x 1]
    Vector3f_t PH;
    matrix3f_mul_vec(&PH, &airspeed_cal->P, &H_TAS);
    float S = vec3_dot(&H_TAS, &PH) + 1.0f;

    // Calculate the Kalman gain
    // [3 x 3] * [3 x 1] / [1 x 1]
    Vector3f_t KG;
    vec3_mult(&KG, &PH, 1.0f/ S);

    // Update the states
    airspeed_cal->state.x += KG.x*(TAS_mea - TAS_pred); // [3 x 1] + [3 x 1] * [1 x 1]
    airspeed_cal->state.y += KG.y*(TAS_mea - TAS_pred); // [3 x 1] + [3 x 1] * [1 x 1]
    airspeed_cal->state.z += KG.z*(TAS_mea - TAS_pred); // [3 x 1] + [3 x 1] * [1 x 1]

    // Update the covariance matrix
    Vector3f_t colx = {airspeed_cal->P.a.x, airspeed_cal->P.b.x, airspeed_cal->P.c.x};
    Vector3f_t coly = {airspeed_cal->P.a.y, airspeed_cal->P.b.y, airspeed_cal->P.c.y};
    Vector3f_t colz = {airspeed_cal->P.a.z, airspeed_cal->P.b.z, airspeed_cal->P.c.z};

    Vector3f_t HP2 = {vec3_dot(&H_TAS, &colx), vec3_dot(&H_TAS, &coly), vec3_dot(&H_TAS, &colz)};

    airspeed_cal->P.a.x -= KG.x * HP2.x;
    airspeed_cal->P.a.y -= KG.x * HP2.y;
    airspeed_cal->P.a.z -= KG.x * HP2.z;

    airspeed_cal->P.b.x -= KG.y * HP2.x;
    airspeed_cal->P.b.y -= KG.y * HP2.y;
    airspeed_cal->P.b.z -= KG.y * HP2.z;

    airspeed_cal->P.c.x -= KG.z * HP2.x;
    airspeed_cal->P.c.y -= KG.z * HP2.y;
    airspeed_cal->P.c.z -= KG.z * HP2.z;

    // force symmetry on the covariance matrix - necessary due to rounding
    // errors
    float P12 = 0.5f * (airspeed_cal->P.a.y + airspeed_cal->P.b.x);
    float P13 = 0.5f * (airspeed_cal->P.a.z + airspeed_cal->P.c.x);
    float P23 = 0.5f * (airspeed_cal->P.b.z + airspeed_cal->P.c.y);
    airspeed_cal->P.a.y = airspeed_cal->P.b.x = P12;
    airspeed_cal->P.a.z = airspeed_cal->P.c.x = P13;
    airspeed_cal->P.b.z = airspeed_cal->P.c.y = P23;

    // Constrain diagonals to be non-negative - protects against rounding errors
    airspeed_cal->P.a.x = MAX(airspeed_cal->P.a.x, 0.0f);
    airspeed_cal->P.b.y = MAX(airspeed_cal->P.b.y, 0.0f);
    airspeed_cal->P.c.z = MAX(airspeed_cal->P.c.z, 0.0f);

    airspeed_cal->state.x = math_constrain_float(airspeed_cal->state.x, -max_airspeed_allowed_during_cal, max_airspeed_allowed_during_cal);
    airspeed_cal->state.y = math_constrain_float(airspeed_cal->state.y, -max_airspeed_allowed_during_cal, max_airspeed_allowed_during_cal);
    airspeed_cal->state.z = math_constrain_float(airspeed_cal->state.z, 0.5f, 1.0f);

    return airspeed_cal->state.z;
}

/*
  called once a second to do calibration update
 */
void sensor_as_update_calibration2(uint8_t i, const Vector3f_t *vground, int16_t max_airspeed_allowed_during_cal)
{
#if MB_AIRSPEED_AUTOCAL_ENABLE
    sensor_airspeed_t snsr_as = sensor_as_get_singleton();

    if (!snsr_as->param[i].autocal && !snsr_as->calibration_enabled) {
        // auto-calibration not enabled
        return;
    }

    // set state.z based on current ratio, this allows the operator to
    // override the current ratio in flight with autocal, which is
    // very useful both for testing and to force a reasonable value.
    float ratio = math_constrain_float(snsr_as->param[i].ratio, 1.0f, 4.0f);

    snsr_as->state[i].calibration.state.z = 1.0f / sqrtf(ratio);

    // calculate true airspeed, assuming a airspeed ratio of 1.0
    float dpress = MAX(sensor_as_get_differential_pressure(i), 0);
    float true_airspeed = sqrtf(dpress) * get_ahrs_view()->EAS2TAS;

    float zratio = airspeed_calibration_update(&snsr_as->state[i].calibration, true_airspeed, vground, max_airspeed_allowed_during_cal);

    if (isnan(zratio) || isinf(zratio)) {
        return;
    }

    // this constrains the resulting ratio to between 1.0 and 4.0
    zratio = math_constrain_float(zratio, 0.5f, 1.0f);
    snsr_as->param[i].ratio = 1/sq(zratio);
    if (snsr_as->state[i].counter > 60) {
        if (snsr_as->state[i].last_saved_ratio > 1.05f*snsr_as->param[i].ratio ||
            snsr_as->state[i].last_saved_ratio < 0.95f*snsr_as->param[i].ratio) {

            param_t *param = param_get_by_variable2("ARSPD", (void*)&snsr_as->param[i].ratio);
            if (param != NULL) {
                param_set_and_save(param, snsr_as->param[i].ratio);
            }

            snsr_as->state[i].last_saved_ratio = snsr_as->param[i].ratio;
            snsr_as->state[i].counter = 0;
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u ratio reset: %f", i , (double)(snsr_as->param[i].ratio));
        }
    } else {
        snsr_as->state[i].counter++;
    }
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
}

/*
  called once a second to do calibration update
 */
void sensor_as_update_calibration(const Vector3f_t *vground, int16_t max_airspeed_allowed_during_cal)
{
    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
        sensor_as_update_calibration2(i, vground, max_airspeed_allowed_during_cal);
    }
    sensor_as_send_airspeed_calibration(vground);
}

void sensor_as_send_airspeed_calibration(const Vector3f_t *vground)
{
#if MB_AIRSPEED_AUTOCAL_ENABLE
    sensor_airspeed_t snsr_as = sensor_as_get_singleton();

    const mavlink_airspeed_autocal_t packet = {
        .vx = vground->x,
        .vy = vground->y,
        .vz = vground->z,
        .diff_pressure = sensor_as_get_differential_pressure(snsr_as->primary),
        .EAS2TAS = get_ahrs_view()->EAS2TAS,
        .ratio = snsr_as->param[snsr_as->primary].ratio,
        .state_x = snsr_as->state[snsr_as->primary].calibration.state.x,
        .state_y = snsr_as->state[snsr_as->primary].calibration.state.y,
        .state_z = snsr_as->state[snsr_as->primary].calibration.state.z,
        .Pax = snsr_as->state[snsr_as->primary].calibration.P.a.x,
        .Pby = snsr_as->state[snsr_as->primary].calibration.P.b.y,
        .Pcz = snsr_as->state[snsr_as->primary].calibration.P.c.z
    };
    gcs_send_to_active_channels(MAVLINK_MSG_ID_AIRSPEED_AUTOCAL,
                                  (const char *)&packet);
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
}
/*------------------------------------test------------------------------------*/


